#include <stdio.h>      /* printf, scanf, puts, NULL */
#include <stdlib.h>     /* srand, rand */
#include <time.h>       /* time */
#include <iostream>
#include "field.h"
#include "node.h"
#include <math.h>

//x is Rows, y is columns

namespace nav
{
//if this value is activated- the map is filled with random obsticles
#define DEBUG_ACTIVE true

#define max(a,b) std::max(a,b)
#define min(a,b) std::min(a,b)
#define for_each_item(item, list) \
    for(T * item = list->head; item != NULL; item = item->next)

#define forTwo(iStart,iMax,jStart,jMax,Operation) for(int i=iStart;i<iMax;i++){for(int j=jStart;j<jMax;j++){Operation}}

//Method takes to positions and transfers into a direction Enum
dirEncoded field::posToDir(int x, int y, int xx, int yy)
{
    int LR=x-xx;
    int UPLOW=y-yy;

    switch (LR)
    {
    case 1:
    {
        if (UPLOW==0) return L;
        if (UPLOW==1) return LOWL;
        if (UPLOW==-1) return UPL;
        break;
    }

    case 0:
    {

        if (UPLOW==1) return LOW;
        if (UPLOW==-1) return UP;
        break;
    }

    case -1:
    {
        if (UPLOW==0) return R;
        if (UPLOW==1) return LOWR;
        if (UPLOW==-1) return UPR;

        break;
    }
    default:
    {
        printf("field::coordToDir Went Wrong");
    }
    }

    return UP;
}

//Method sets this fields Node at x y to a particular Weight
void field::setNodeWeight(int x, int y, int Weight)
{
    storage[x+ sizeX*y]->weight=Weight;
}

//Method sets square and wall parts of the map Obsticle
void field::RandoMazeField(int nrOfSolids, int solidSize, int nrOfWalls, int wallSize, int playerX, int playerY, int targetX, int targetY)
{
    std::cout<<"Got this far "<< 0 <<std::endl;
    int xStart;
    int xEnd  ;
    int yStart ;
    int yEnd    ;
    xStart=1;
    xEnd =1 ;
    yStart =1;
    yEnd   =1 ;

    forTwo(0,this->sizeX,0,this->sizeY,this->getNodeAt(i,j)->weight=OBSTICLE;)
    std::cout<<"Got this far "<< 1 <<std::endl;

    for (int i=0; i< nrOfSolids*7; i++)
    {
        //dice Start
        xStart =abs(rand()%this->sizeX);
        xEnd   =clamp(xStart+rand()%(solidSize/4),sizeX);
        yStart ==clamp(yStart+rand()%(solidSize/4),sizeY);
        yEnd   =rand()%this->sizeY;
        //dice End
        SetSquareObsticle(xStart,yStart,xEnd,yEnd,false);
    }

    xStart=1;
    xEnd =1 ;
    yStart =1;
    yEnd   =1 ;
    std::cout<<"Got this far "<< 2 <<std::endl;
    for (int i=0; i< nrOfWalls; i++)
    {
        //dice Start

        xStart=rand()%this->sizeX;

        if (rand()%2==1)
        {
            //xAxis
            xEnd    =clamp(xStart+rand()%wallSize,sizeX);
            yStart  =rand()%(this->sizeY);
            yEnd    ==yStart;
        }
        else
        {
            //zAxis
            yStart  =rand()% ( this->sizeY);
            std::cout<<yStart<<std::endl;
            yEnd    =clamp(yStart+rand()%wallSize,sizeY);
            xEnd    ==xStart;
        }
        std::cout<<"Got this far "<<3 <<std::endl;
        SetSquareObsticle(xStart,yStart,xEnd,yEnd,false);
        std::cout<<"Got this far "<<4 <<std::endl;
        //dice End

    }
    std::cout<<"Got this far "<< 5 <<std::endl;
    SetSquareObsticle(0,sizeY/2,sizeX,sizeY/2,false);
    SetSquareObsticle(sizeX/2,0,sizeX/2,sizeY,false);
    SetSquareObsticle(clamp(playerX-5,sizeX),clamp(playerY-5,sizeY),clamp(playerX+5,sizeX),clamp(playerY+5,sizeY),false);
    SetSquareObsticle(clamp(targetX-5,sizeX),clamp(targetY-5,sizeY),clamp(targetX+5,sizeX),clamp(targetY+5,sizeY),false);

}

void field::SetSquareObsticle(int xStart, int yStart, int xEnd, int yEnd,bool NoObsticle)
{
    std::cout<<"SetSquareObsticleTest" << xStart << " >> "<< xEnd << " >> "<< yStart << " >>"<<yEnd <<std::endl;
    if (NoObsticle)
    {

        forTwo(xStart,xEnd,yStart,yEnd,this->getNodeAt(i,j)->weight=OBSTICLE;)
    }
    else
    {

        forTwo(xStart,xEnd,yStart,yEnd,this->getNodeAt(i,j)->weight=0;)
    }
}

field::field( int sizeX, int sizeY)
{
    this->storage.resize(sizeX*sizeY, new Node(0,0,0,false));
    this->sizeX=sizeX;
    this->sizeY=sizeY;

    for (int i=0; i< sizeX; i++)
    {
        for (int j=0; j< sizeY; j++)
        {
            Node * NodeCopyPtr =  new Node(i,j,0,false);
            NodeCopyPtr->weight=0;
            NodeCopyPtr->tweight=0;
            NodeCopyPtr->predecessorPtr=NULL;

            this->storage[i+ (sizeX*j)]=NodeCopyPtr;
        }
    }

//Field SetUp completed
}


Node* field::getNodeAt(int x, int y)
{
    return storage[sizeX*y+x];
}



//
void field::setNodeAt(int x, int y, Node *NodeToSet)
{
    if (NodeToSet->infoPath==true && getNodeAt(x,y)->weight==OBSTICLE )
    {

        printf("I do object, i do object, node is a OBSTICLE!\n x %d y %d",x,y);
        //Sleep(2000);
    }
    if ( x != NodeToSet->posX || y != NodeToSet->posY )
    {

        printf("Node mapped to wrong position\n x %d y %d",x,y);
        //Sleep(2000);
    }
    this->storage[this->sizeX*y+x]=NodeToSet;
}

//Method renders the field
void field::displayMap(int x, int y, int targetX, int targetY)
{

    system("cls");
    bool Start=false;
    bool Target=false;
    bool Path=false;

    for (int k=0; k <this->sizeX+4; k++)
    {
        printf("=");
    };
     printf("\n");

    //Drawing the Map
    for (int i=0; i< this->sizeX; i++)
    {
        printf("||");//Border

        for (int j=0; j< this->sizeY; j++)
        {
            if (x==i&& y==j)
            {
                Start=true;
            }
            else
            {
                Start=false;
            }

            if (targetX==i&& targetY==j)
            {
                Target=true;
            }
            else
            {
                Target=false;
            }

            if(getNodeAt(i,j)->infoPath==true)
            {
                Path=true;
            }
            else
            {
                Path=false;
            }

            printf("%c", MapIntToChar(this->getNodeAt(i,j)->weight,Start,Target, Path));

        }
        printf("||\n");
    }
    for (int k=0; k <this->sizeX+4; k++)
    {
        printf("=");
    };
     printf("\n");
    //Taking a nap
//Sleep(500);
}

//Helper Method for displayMap
char field::MapIntToChar(int mapVal,bool startCoords, bool targCoords,bool path)
{
    if (targCoords==true) return '+';
    if (startCoords==true )return '@';

    if (mapVal > OBSTICLE>>1) return 'O';
    if (path==true )return '*';
    if (mapVal > OBSTICLE>>2) return '#';
    if (mapVal > OBSTICLE>>3) return ':';
    if (mapVal > OBSTICLE>>4) return '.';
    return ' ';

}

/*!Explanation
 Here is were it gets interesting, every Move and every Turn is applied to the whole known Map as a "Blur Effect"
 Meaning the Map is distorted into the direction of movement or
 in case of a Turn orthogonal to the direction the unit turns towards multiplied by a factor that represents the lenght of turn
 The actuall distortion strenght should be a heuristic, determinated during Test-Runs
 The traditional, less cpu-intensive approach of bluring the robots position is exchanged for the benefit of having the obsticles
 blur by distance and time of discovery.

 If time and ressources allow it, the robot could recalibrate on the rocks, allowing for a "sharpening of the map along one axis.
*/


//TODO UNTESTED METHOD
void field::applyUPDistortion(int distortion)
{
    for (int i=0; i< sizeX; i++)
    {
        for (int j=1; j < sizeY-1; j++)
        {

            if ( getNodeAt(i,j)->weight!= OBSTICLE)
            {
                setNodeWeight(i,j,(getNodeAt(i,j)->weight+ getNodeAt(i,j-1)->weight/4+ getNodeAt(i,j+1)->weight/4) );
            }
        }
    }

}

//TODO UNTESTED METHOD
void field::applyUPRDistortion(int distortion)
{
    for (int i=1; i< sizeX-1; i++)
    {
        for (int j=1; j < sizeY-1; j++)
        {

            if ( getNodeAt(i,j)->weight!= OBSTICLE)
            {
                setNodeWeight(i,j,(getNodeAt(i,j)->weight+ getNodeAt(i-1,j+1)->weight/4+ getNodeAt(i+1,j-1)->weight/4) );
            }
        }
    }


};

//TODO UNTESTED METHOD
void field::applyUPLDistortion(int distortion)
{
    for (int i=0; i< sizeX; i++)
    {
        for (int j=1; j < sizeY-1; j++)
        {
            //TODO replace
            if ( getNodeAt(i,j)->weight!= OBSTICLE)
            {
                setNodeWeight(i,j,(getNodeAt(i,j)->weight+ getNodeAt(i-1,j-1)->weight/4+ getNodeAt(i,j+1)->weight/4) );
            }
        }
    }

};

//TODO UNTESTED METHOD
void field:: applyLRDistortion(int distortion)
{
    for (int i=0; i< sizeX; i++)
    {
        int j=1;
        while (j+1 < sizeY)
        {
            if ( getNodeAt(j+i,j)->weight != OBSTICLE)
                setNodeWeight(j+i,j,(getNodeAt(j+i,j)->weight+ getNodeAt(j+i-1,j-1)->weight/4+ getNodeAt(j+i+1,j+1)->weight/4) );
            j++;
        }
    }

    for (int i=1; i< sizeY; i++)
    {
        int j=1;
        while (j+1 < sizeY)
        {
            if ( getNodeAt(i,j)->weight!= OBSTICLE)
                setNodeWeight(i+j,j,(getNodeAt(i+j,j)->weight+ getNodeAt(i-1+j,j-1)->weight/4+ getNodeAt(i+1+j,j+1)->weight/4) );
            j++;
        }
    }

}

void field::resetDebugInfo()
{
    Node* tempNodePtr;
    for (int i=0; i< this->sizeX; i++)
    {
        for (int j=0; j< this->sizeY; j++)
        {
            tempNodePtr=this->getNodeAt(i,j);
            tempNodePtr->infoPath=false;
        }
    }
}



void field::applyDistortion(dirEncoded direction, int distortion)
{
    switch (direction)
    {
    case LOW:
    case UP :
    {
        applyUPDistortion(distortion);
        break;
    }
    case UPR:
    case LOWL:
    {
        applyUPRDistortion(distortion);
        break;
    }
    case UPL:
    case LOWR:
    {
        applyUPLDistortion(distortion);
        break;
    }
    case L:
    case R:
    {
        applyLRDistortion(distortion);
        break;
    }
    default:
        break;
    }
}

//Helper Method, that confines a value to a intervall
int field::clamp(int val, int upLimit)
{
    if (val < 0 ) return 0;
    if (val > upLimit) return upLimit;
    return val;
}


//Method extracts the Neighbours of a Node- filtering out Nodes that are OBSTICLES
std::vector<Node*> field::getNeighbours(Node *observedNode)
{

    //the vector which we return as result
    std::vector<Node*> returnToSender;
    int count=0;

    // we itterate over the field from one less to one more then the current Nodes Pos
    for (int x=clamp(observedNode->posX-1,sizeX); x <= clamp(observedNode->posX+1, sizeX); x++)
    {

        for (int y=clamp(observedNode->posY-1,sizeY); y <= clamp(observedNode->posY+1,sizeY); y++)
        {

            // printf("Neighbour weight"<<this->getNodeAt(x,y)->weight);
            // if the Node is not the observed Node
            if( (x != observedNode->posX || y!= observedNode->posY)  && (this->getNodeAt(x,y)->weight < OBSTICLE) )
            {
                //Troublemaker
                count++;
                //   printf(count<<" Push Node at Neighbours"<<(this->getNodeAt(x,y)->weight));
                returnToSender.push_back(this->getNodeAt(x,y));
            }
        }
    }



    return returnToSender;
};

//Destructor
field::~field()
{


    for (int i=0; i< sizeX; i++)
    {
        for (int j=0; j< sizeY; j++)
        {
            delete (this->storage[i+ (sizeX*j)]);
        }
    }

}

//dtor



}
